function [X,Y,car_angle,Xd,Yd,car_angle_d]=cor_cal2(v,w,vd,wd,dt)
    coder.inline("never");
    % 初始化静态变量
    persistent X0;
    if isempty(X0)
            X0=0;
    end
    persistent Y0;
    if isempty(Y0)
            Y0=0;
    end
    persistent sig0;
    if isempty(sig0)
            sig0=0;
    end
    %P上一级
    Pk_1=[X0;Y0;sig0];
    angM=[cos(sig0),0;
        sin(sig0),0;
        0,1];
    %P更新
    Pk=Pk_1+dt*angM*[v;w];
    X=Pk(1);
    Y=Pk(2);
    car_angle=Pk(3);
    %P预测更新
    Pk_d=Pk_1+dt*angM*[vd;wd];
    Xd=Pk_d(1);
    Yd=Pk_d(2);
    car_angle_d=Pk_d(3);
%     Rd=vd/wd;
%     dL_d=abs(Rd*sin(wd*dt/2)*2);
%     d_angle=sig0+wd*dt/2;
%     Xd=X0+dL_d*cos(d_angle);
%     Yd=Y0+dL_d*sin(d_angle);
%     car_angle_d=sig0+wd*dt;
    %静态变量更新
    X0=X;
    Y0=Y;
    sig0=car_angle;
end